#!/usr/bin/python
# -*- coding: utf-8 -*-


import pygame
import random
import yaml
import io
import logging
import math

from pygame.locals import *

logging.basicConfig(level=logging.INFO)

PARTICLE_COUNT=500
b=0.3

FORWARD_SPEED=0.2 
    

class Map:
    def __init__(self):
        self.particleList=[]
       
        self.loadMap()
        #self.initParticle() 
        
        self.robot=Robot(self,100,300,0,(0.3,0.3),(255,0,0),True)


    def initParticle(self):
        for t in range(1,PARTICLE_COUNT):
            particle=Robot(self,random.randint(20,580),random.randint(20,580),random.uniform(0,math.pi*2),(0.3,0.3),(0,255,0)) 
            self.particleList.append(particle)
         

    def loadMap(self):
       
        mapYAML=self.loadMapFile()
        self.resolution=mapYAML['resolution']        
        
        self.loadMapImage(mapYAML['image'])    
    def loadMapImage(self,img):
        logging.info('Load image map : %s',img)
           
        self.mapImage = pygame.image.load(img)
       
        width,height=self.mapImage.get_size()
        
        self.mapData=[] 
        for i in range(0,height):
            a=[] 
            for j in range(0,width):
                if self.mapImage.get_at((i,j))==(255,255,255,255):
                   a.append(0)
                else:
                   a.append(1)
            self.mapData.append(a)   
 
        
         
                        
    def loadMapFile(self):
        
        logging.info("Load map file")
        try:
            fh=io.open('map.yaml') 
            try:
               mapYAML=yaml.load(fh)
            finally:
               fh.close() 
        except IOError:
           print "Error read file"

        return mapYAML
    
    def update(self,dt):
        self.robot.update(dt)
        for p in self.particleList:
            p.update(dt)   

        #print self.robot.x 
        #if self.mapData[self.robot.x][self.robot.y]==1 :
        #   print "a"              
  

    def draw(self,surface):

        # draw background
        surface.blit(self.mapImage,(0,0))   
        
        for p in self.particleList:
            p.draw(surface)
        self.robot.draw(surface) 

class Robot:
    def __init__(self,_map,x,y,orientation,size,color,showSonar=False):
 
        self.x=x
        self.y=y
        self.orientation=orientation
        self._map=_map
        self.color=color
        self.showSonar=showSonar
        
        self.vr=float(FORWARD_SPEED/self._map.resolution)
        self.vl=float(FORWARD_SPEED/self._map.resolution)

        self.size=(size[0]/self._map.resolution,size[1]/self._map.resolution)

        self.b=float(b)/self._map.resolution

        self.surface=pygame.Surface(self.size,pygame.HWSURFACE | pygame.SRCALPHA) 

        self.tempSurface=self.surface         
 
        self.sonarList=[Sonar(_map,0,3,(255,0,0)),Sonar(_map,math.pi/2),Sonar(_map,math.pi*3/2)]     
 
    def set(self,leftV,rightV):
        self.leftV=leftV
        self.rightV=rightV    
    
    def update(self,dt):
        


        linearSpeed=(self.vr+self.vl)/2
        angularZ=(self.vr-self.vl)/self.b 

        #print angularZ
           
        self.x+=linearSpeed*dt*math.cos(self.orientation)
        self.y-=linearSpeed*dt*math.sin(self.orientation)

        

        if self.sonarList[1].actualRange<=30:
           self.vr=-1*FORWARD_SPEED/self._map.resolution*random.uniform(0.5,1.5)
           self.vl=-1*FORWARD_SPEED/self._map.resolution*random.uniform(0.5,1.5)

        print self.vr
 
        self.orientation+=angularZ*dt

        #logging.info("%f - %f",self.x,self.y)
        #logging.info("%f - %f",vx,vy)
        

        #self.x=int(self.x)
        #self.y=int(self.y) 
           
        sWidth,sHeight=self.tempSurface.get_size()
        

        for s in self.sonarList:
            s.update(self.x+sWidth/2,self.y+sHeight/2,self.orientation)

        #print self.sonarList[1].actualRange 
        
        
    def draw(self,surface):
      
        #pygame.draw.rect(surface,(255,0,0),(self.x-10,self.y-10,20,20),0)
        pygame.draw.rect(self.surface,self.color,(0,0,self.size[0],self.size[1]),0)
        self.tempSurface=pygame.transform.rotate(self.surface,self.orientation*180/math.pi)
        #pygame.draw.rect(self.tempSurface,(0,0,255),(0,0,self.tempSurface.get_size()[0],self.tempSurface.get_size()[1]),1) 
        surface.blit(self.tempSurface,(int(self.x),int(self.y)))  
        
        if self.showSonar :  
           for s in self.sonarList:
               s.draw(surface)  
               


class Sonar:
      def __init__(self,_map,orientation,_range=3,color=(0,0,255)):
 
          self._map=_map
          self.orientation=orientation
          self.absoluteOrientation=orientation
          self._range=_range  
          self.xEnd=0
          self.yEnd=0
          self.color=color
          self.actualRange=_range/self._map.resolution
           
      
      def update(self,x,y,orientation):
          self.x=int(x)
          self.y=int(y)
          self.absoluteOrientation=(self.orientation+orientation) % (math.pi*2)
         
          self.xEnd=self.x+int(math.sin(self.absoluteOrientation)*self._range/self._map.resolution)
          self.yEnd=self.y+int(math.cos(self.absoluteOrientation)*self._range/self._map.resolution) 
            
 
          coords=self.bresenham_line((self.x,self.y),(self.xEnd,self.yEnd))
          
       
          for c in coords:
              if c[0]>=600 or c[1]>=600:
                 break   
               
              if self._map.mapData[c[0]][c[1]]==1:
                 self.xEnd=c[0]
                 self.yEnd=c[1]
                 
                 break
          
          self.actualRange=math.sqrt((self.x-self.xEnd)*(self.x-self.xEnd)+(self.y-self.yEnd)*(self.y-self.yEnd))

         

      def draw(self,surface):
          #pygame.draw.line(surface,(255,255,0),(self.x,self.y),(5,500),1)
          pygame.draw.line(surface,self.color,(self.x,self.y),(self.xEnd,self.yEnd),4)
          
      def bresenham_line(self,(x,y),(x2,y2)):
          """Brensenham line algorithm"""
          steep = 0
          coords = []
          dx = abs(x2 - x)
          if (x2 - x) > 0: 
             sx = 1
          else: 
             sx = -1
          dy = abs(y2 - y)
          if (y2 - y) > 0:
             sy = 1
          else: 
             sy = -1
         
          if dy > dx:
             steep = 1
             x,y = y,x
             dx,dy = dy,dx
             sx,sy = sy,sx
    
          d = (2 * dy) - dx
          for i in range(0,dx):
             if steep: 
                coords.append((y,x))
             else: 
                coords.append((x,y))
             
             while d >= 0:
                 y = y + sy
                 d = d - (2 * dx)
             
             x = x + sx
             d = d + (2 * dy)
         
          coords.append((x2,y2)) 
          return coords

 
class App:
    def __init__(self):
        self._running = True
        self._display_surf = None
        self._image_surf = None
        self.map=Map()
        self.lastTime=float(pygame.time.get_ticks()) 
 
    def on_init(self):
        pygame.init()
        self._display_surf = pygame.display.set_mode((600,600), pygame.HWSURFACE)
        self._running = True
        #iself._image_surf = pygame.image.load("myimage.jpg").convert()

        #self.particleList=[Particle(200,200),Particle(250,100)]
 
    def on_event(self, event):
        if event.type == QUIT:
            self._running = False
    def on_loop(self):
        
        currentTime=float(pygame.time.get_ticks())
      
        dt=(currentTime-self.lastTime)/1000
        self.lastTime=currentTime
        self.map.update(dt)

    def on_render(self):
        #self._display_surf.blit(self._image_surf,(0,0))
        #pygame.draw.line(self._display_surf,(255,255,255),(0,0),(100,100)) 
        self.map.draw(self._display_surf)

        pygame.display.flip()
 
    def on_cleanup(self):
        pygame.quit()
 
    def on_execute(self):
        if self.on_init() == False:
            self._running = False
 
        while( self._running ):
            for event in pygame.event.get():
                self.on_event(event)
            self.on_loop()
            self.on_render()
        self.on_cleanup()
 
if __name__ == "__main__" :
    theApp = App()
    theApp.on_execute()






